/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2021-10-14     Administrator       the first version
 *
 *
 *
 */

#include <rtthread.h>
#include <rtdevice.h>
#include <stdlib.h>
#include "ps2.h"
#include "motor.h"
#define THREAD_PRIORITY         16
#define THREAD_STACK_SIZE       512
#define THREAD_TIMESLICE        10

#define PWM_DEV_NAME1        "pwm1"  /* PWM设备名称 */
#define PWM_DEV_NAME2        "pwm2"  /* PWM设备名称 */
#define PWM_DEV_CHANNEL1     1       /* PWM通道 */
#define PWM_DEV_CHANNEL2     2       /* PWM通道 */
#define PWM_DEV_CHANNEL3     3       /* PWM通道 */
#define PWM_DEV_CHANNEL4     4       /* PWM通道 */

static rt_thread_t tid1 = RT_NULL;
struct rt_device_pwm *pwm_dev1; /* PWM设备句柄 */
struct rt_device_pwm *pwm_dev2; /* PWM设备句柄 */

extern JOYSTICK_TypeDef my_joystick;
rt_uint32_t pulset = 300000;
rt_uint32_t period, dir;

//常量定义
#define ROBOT_AB  1   //车体尺寸，车轮距离中心距之和

rt_int32_t ax_motor_pwm[4];  //电机PWM速度0-A，1-B,2-C,3-D

rt_int32_t ax_vx; //X轴运动速度，控制横向移动
rt_int32_t ax_vy; //Y轴运动速度，控制前后移动
rt_int32_t ax_vz; //Z轴运动速度，控制转向

void stop(void)
{
    rt_pwm_set(pwm_dev1, PWM_DEV_CHANNEL1, period, 0);
    rt_pwm_set(pwm_dev1, PWM_DEV_CHANNEL2, period, 0);
    rt_pwm_set(pwm_dev1, PWM_DEV_CHANNEL3, period, 0);
    rt_pwm_set(pwm_dev1, PWM_DEV_CHANNEL4, period, 0);
    rt_pwm_set(pwm_dev2, PWM_DEV_CHANNEL1, period, 0);
    rt_pwm_set(pwm_dev2, PWM_DEV_CHANNEL2, period, 0);
    rt_pwm_set(pwm_dev2, PWM_DEV_CHANNEL3, period, 0);
    rt_pwm_set(pwm_dev2, PWM_DEV_CHANNEL4, period, 0);
    rt_thread_mdelay(100);
}

void move_test(void)
{
    rt_uint32_t pulse = 200000;
    rt_pwm_set(pwm_dev1, PWM_DEV_CHANNEL1, period, pulse); //左前轮正传
    rt_pwm_set(pwm_dev1, PWM_DEV_CHANNEL2, period, 0);

    rt_pwm_set(pwm_dev1, PWM_DEV_CHANNEL4, period, pulse); //右前轮正传
    rt_pwm_set(pwm_dev1, PWM_DEV_CHANNEL3, period, 0);

    rt_pwm_set(pwm_dev2, PWM_DEV_CHANNEL2, period, 0);
    rt_pwm_set(pwm_dev2, PWM_DEV_CHANNEL1, period, pulse); //左后轮正传

    rt_pwm_set(pwm_dev2, PWM_DEV_CHANNEL4, period, 0);
    rt_pwm_set(pwm_dev2, PWM_DEV_CHANNEL3, period, pulse); //右后轮正传
}
MSH_CMD_EXPORT(move_test, motor test);

/**
 * @简  述 电机A PWM速度控制
 * @参  数 speed 电机转速数值，范围-500000~500000
 * @返回值 无
 *
 *         A  @------------@    B
 *                 |  |
 *                 |  |
 *                 |  |
 *                 |  |
 *                 |  |
 *                 |  |
 *                 |  |
 *         C  @------------@    D
 **/
void AX_MOTOR_A_SetSpeed(rt_int32_t speed)
{
    rt_uint32_t temp;
    temp = abs(speed);
    if (temp > 500000) //限定最大速度
        temp = 500000;

    if (speed > 0)
    {
        rt_pwm_set(pwm_dev1, PWM_DEV_CHANNEL4, period, temp); //左前轮正传
        rt_pwm_set(pwm_dev1, PWM_DEV_CHANNEL3, period, 0);

    }
    else if (speed < 0)
    {
        rt_pwm_set(pwm_dev1, PWM_DEV_CHANNEL4, period, 0); //左前轮反传
        rt_pwm_set(pwm_dev1, PWM_DEV_CHANNEL3, period, temp);

    }
    else
    {
        rt_pwm_set(pwm_dev1, PWM_DEV_CHANNEL4, period, 0); //左前轮停止
        rt_pwm_set(pwm_dev1, PWM_DEV_CHANNEL3, period, 0);
    }
}
/**
 * @简  述 电机B PWM速度控制
 * @参  数 speed 电机转速数值，范围-500000~500000
 * @返回值 无
 */
void AX_MOTOR_B_SetSpeed(rt_int32_t speed)
{
    rt_uint32_t temp;
    temp = abs(speed);
    if (temp > 500000) //限定最大速度
        temp = 500000;

    if (speed > 0)
    {
        rt_pwm_set(pwm_dev1, PWM_DEV_CHANNEL1, period, temp); //右前轮正传
        rt_pwm_set(pwm_dev1, PWM_DEV_CHANNEL2, period, 0);

    }
    else if (speed < 0)
    {
        rt_pwm_set(pwm_dev1, PWM_DEV_CHANNEL1, period, 0); //右前轮反传
        rt_pwm_set(pwm_dev1, PWM_DEV_CHANNEL2, period, temp);

    }
    else
    {
        rt_pwm_set(pwm_dev1, PWM_DEV_CHANNEL1, period, 0); //右前轮停止
        rt_pwm_set(pwm_dev1, PWM_DEV_CHANNEL2, period, 0);
    }

}

/**
 * @简  述 电机C PWM速度控制
 * @参  数 speed 电机转速数值，范围-500000~500000
 * @返回值 无
 */
void AX_MOTOR_C_SetSpeed(rt_int32_t speed)
{

    rt_uint32_t temp;
    temp = abs(speed);
    if (temp > 500000) //限定最大速度
        temp = 500000;

    if (speed > 0)
    {
        rt_pwm_set(pwm_dev2, PWM_DEV_CHANNEL2, period, 0);
        rt_pwm_set(pwm_dev2, PWM_DEV_CHANNEL1, period, temp); //左后轮正传

    }
    else if (speed < 0)
    {
        rt_pwm_set(pwm_dev2, PWM_DEV_CHANNEL2, period, temp);
        rt_pwm_set(pwm_dev2, PWM_DEV_CHANNEL1, period, 0); //左后轮反传
    }
    else
    {
        rt_pwm_set(pwm_dev2, PWM_DEV_CHANNEL2, period, 0);
        rt_pwm_set(pwm_dev2, PWM_DEV_CHANNEL1, period, 0); //左后轮停止
    }
}

/**
 * @简  述 电机D PWM速度控制
 * @参  数 speed 电机转速数值，范围-500000~500000
 * @返回值 无
 */
void AX_MOTOR_D_SetSpeed(rt_int32_t speed)
{
    rt_uint32_t temp;
    temp = abs(speed);
    if (temp > 500000) //限定最大速度
        temp = 500000;

    if (speed > 0)
    {
        rt_pwm_set(pwm_dev2, PWM_DEV_CHANNEL4, period, 0);
        rt_pwm_set(pwm_dev2, PWM_DEV_CHANNEL3, period, temp); //右后轮正传

    }
    else if (speed < 0)
    {
        rt_pwm_set(pwm_dev2, PWM_DEV_CHANNEL4, period, temp);
        rt_pwm_set(pwm_dev2, PWM_DEV_CHANNEL3, period, 0); //右后轮反传

    }
    else
    {
        rt_pwm_set(pwm_dev2, PWM_DEV_CHANNEL4, period, 0);
        rt_pwm_set(pwm_dev2, PWM_DEV_CHANNEL3, period, 0); //右后轮停止
    }
}

rt_uint16_t ka = 500, kb = 500, kc = 500, kd = 500;
rt_uint16_t K = 2000;
extern rt_int32_t count_b, count_c;
/**
 * @简  述  由坐标XYZ速度解析为电机转速
 * @参  数  vx，vy，vz  三轴坐标速度
 * @返回值  全局变量，每个轮子PWM速度
 */
void AX_MOVE_Kinematics(rt_uint32_t pulse)
{
    static rt_int32_t count_bt = 0;
    static rt_int32_t count_ct = 0;
    count_bt = 150;
    count_ct = 150;

    if (my_joystick.mode != 0x41)
    {
        count_bt = 0;
        count_ct = 0;
        ax_motor_pwm[0] = ((count_bt) * K + ka * (count_bt - count_b));
        ax_motor_pwm[1] = ((count_bt) * K + kb * (count_bt - count_b));
        ax_motor_pwm[2] = ((count_ct) * K + kc * (count_ct - count_c));
        ax_motor_pwm[3] = ((count_ct) * K + kd * (count_ct - count_c));
        return;

    }
    if (my_joystick.btn1 == 0x7f) //左自旋转
    {
        ax_motor_pwm[0] = -(((count_bt) * K + kb * (count_bt - count_b)) + 20000); //A
        ax_motor_pwm[1] = ((count_bt) * K + kb * (count_bt - count_b)) + 20000;  //B
        ax_motor_pwm[2] = -((count_ct) * K + kc * (count_ct - count_c)); //C
        ax_motor_pwm[3] = ((count_ct) * K + kc * (count_ct - count_c));  //D
    }
    else if (my_joystick.btn1 == 0xdf)  //右自旋转
    {
        ax_motor_pwm[0] = ((count_bt) * K + ka * (count_bt - count_b)) + 20000;  //A
        ax_motor_pwm[1] = -(((count_bt) * K + kb * (count_bt - count_b)) + 20000); //B
        ax_motor_pwm[2] = ((count_ct) * K + kc * (count_ct - count_c));  //C
        ax_motor_pwm[3] = -((count_ct) * K + kc * (count_ct - count_c)); //D
    }

    else if (my_joystick.btn1 == 0xef) //左横走
    {
        ax_motor_pwm[0] = -(((count_bt) * K + ka * (count_bt - count_b)) + 20000); //A
        ax_motor_pwm[1] = ((count_bt) * K + kb * (count_bt - count_b)) + 20000; //B
        ax_motor_pwm[2] = ((count_ct) * K + kc * (count_ct - count_c));; //C
        ax_motor_pwm[3] = -((count_ct) * K + kd * (count_ct - count_c)); //D
    }
    else if (my_joystick.btn1 == 0xbf) //右横走
    {

        ax_motor_pwm[0] = ((count_bt) * K + ka * (count_bt - count_b)) + 20000; //A
        ax_motor_pwm[1] = -(((count_bt) * K + ka * (count_bt - count_b)) + 20000); //B
        ax_motor_pwm[2] = -((count_ct) * K + kc * (count_ct - count_c)); //C
        ax_motor_pwm[3] = ((count_ct) * K + kd * (count_ct - count_c)); //D
    }
    else if (my_joystick.btn2 == 0x7f) //左转
    {
        ax_motor_pwm[0] = ((count_bt) * K + ka * (count_bt - count_b)); //A
        ax_motor_pwm[1] = ((count_bt) * K + kb * (count_bt - count_b)) + 100000; //B
        ax_motor_pwm[2] = ((count_ct) * K + kc * (count_ct - count_c)); //C
        ax_motor_pwm[3] = ((count_ct) * K + kd * (count_ct - count_c)) + 100000; //D
    }

    else if (my_joystick.btn2 == 0xdf) //右转
    {
        ax_motor_pwm[0] = ((count_bt) * K + ka * (count_bt - count_b)) + 100000; //A
        ax_motor_pwm[1] = ((count_bt) * K + kb * (count_bt - count_b)); //B
        ax_motor_pwm[2] = ((count_ct) * K + kc * (count_ct - count_c)) + 100000; //C
        ax_motor_pwm[3] = ((count_ct) * K + kd * (count_ct - count_c));
        ; //D
    }
    else if (my_joystick.btn2 == 0xef) //前进
    {

        ax_motor_pwm[0] = ((count_bt) * K + ka * (count_bt - count_b));
        ax_motor_pwm[1] = ((count_bt) * K + kb * (count_bt - count_b));
        ax_motor_pwm[2] = ((count_ct) * K + kc * (count_ct - count_c));
        ax_motor_pwm[3] = ((count_ct) * K + kd * (count_ct - count_c));
    }
    else if (my_joystick.btn2 == 0xbf) //后退
    {

        count_bt = 150;
        count_ct = 150;
        ax_motor_pwm[0] = -((count_bt) * K + ka * (count_bt - count_b));
        ax_motor_pwm[1] = -((count_bt) * K + kb * (count_bt - count_b));
        ax_motor_pwm[2] = -((count_ct) * K + kc * (count_ct - count_c));
        ax_motor_pwm[3] = -((count_ct) * K + kd * (count_ct - count_c));

    }
    else
    {
        if (my_joystick.btn2 == 0xfb || my_joystick.btn2 == 0xfe) //加速
        {
            pulset += 2000;
            rt_kprintf("pulset = %d\r\n", pulset);
            return;
        }
        else if (my_joystick.btn2 == 0xf7 || my_joystick.btn2 == 0xfd) //减速
        {
            pulset -= 2000;
            return;
        }
        count_bt = 0;
        count_ct = 0;
        ax_motor_pwm[0] = ((count_bt) * K + ka * (count_bt - count_b));
        ax_motor_pwm[1] = ((count_bt) * K + kb * (count_bt - count_b));
        ax_motor_pwm[2] = ((count_ct) * K + kc * (count_ct - count_c));
        ax_motor_pwm[3] = ((count_ct) * K + kd * (count_ct - count_c));
    }
    rt_memset(&my_joystick, 0, sizeof(my_joystick));
}

static int pwm_init(void)
{

    period = 500000; /* 周期为0.5ms，单位为纳秒ns  频率2K hz*/

    /* 查找设备 */
    pwm_dev1 = (struct rt_device_pwm *) rt_device_find(PWM_DEV_NAME1);
    if (pwm_dev1 == RT_NULL)
    {
        rt_kprintf("can't find %s device!\n", PWM_DEV_NAME1);
        return RT_ERROR;
    }

    pwm_dev2 = (struct rt_device_pwm *) rt_device_find(PWM_DEV_NAME2);
    if (pwm_dev2 == RT_NULL)
    {
        rt_kprintf("can't find %s device!\n", PWM_DEV_NAME2);
        return RT_ERROR;
    }

    /* 设置PWM周期和脉冲宽度默认值 */
    rt_pwm_set(pwm_dev1, PWM_DEV_CHANNEL1, period, 0);
    rt_pwm_set(pwm_dev1, PWM_DEV_CHANNEL2, period, 0);
    rt_pwm_set(pwm_dev1, PWM_DEV_CHANNEL3, period, 0);
    rt_pwm_set(pwm_dev1, PWM_DEV_CHANNEL4, period, 0);
    rt_pwm_set(pwm_dev2, PWM_DEV_CHANNEL1, period, 0);
    rt_pwm_set(pwm_dev2, PWM_DEV_CHANNEL2, period, 0);
    rt_pwm_set(pwm_dev2, PWM_DEV_CHANNEL3, period, 0);
    rt_pwm_set(pwm_dev2, PWM_DEV_CHANNEL4, period, 0);
    rt_pwm_enable(pwm_dev1, PWM_DEV_CHANNEL1);
    rt_pwm_enable(pwm_dev1, PWM_DEV_CHANNEL2);
    rt_pwm_enable(pwm_dev1, PWM_DEV_CHANNEL3);
    rt_pwm_enable(pwm_dev1, PWM_DEV_CHANNEL4);
    rt_pwm_enable(pwm_dev2, PWM_DEV_CHANNEL1);
    rt_pwm_enable(pwm_dev2, PWM_DEV_CHANNEL2);
    rt_pwm_enable(pwm_dev2, PWM_DEV_CHANNEL3);
    rt_pwm_enable(pwm_dev2, PWM_DEV_CHANNEL4);
    return RT_EOK;

}

static void thread1_entry(void *parameter)
{

    pwm_init();
    rt_thread_mdelay(10);

    while (1)
    {

        AX_MOVE_Kinematics(pulset);
//  电机执行动作

        AX_MOTOR_A_SetSpeed(ax_motor_pwm[0]);
        AX_MOTOR_B_SetSpeed(ax_motor_pwm[1]);
        AX_MOTOR_C_SetSpeed(ax_motor_pwm[2]);
        AX_MOTOR_D_SetSpeed(ax_motor_pwm[3]);
        rt_thread_mdelay(50);
    }
}
int thread_motor_init(void)
{
    /* 创建线程1，名称是thread1，入口是thread1_entry*/
    tid1 = rt_thread_create("motor", thread1_entry, RT_NULL,
    THREAD_STACK_SIZE,
    THREAD_PRIORITY, THREAD_TIMESLICE);

    /* 如果获得线程控制块，启动这个线程 */
    if (tid1 != RT_NULL)
        rt_thread_startup(tid1);

    return 0;
}

INIT_APP_EXPORT(thread_motor_init);
